ROS2 gazebo (classic) ultrasonic sensor
Table of Content
- Create simple model with ultrasonic sensor
- Spawn the model into gazebo world
- View sensor output in RVIZ
sdf / sensor#
ultrasonic/model.sdf
<?xml version="1.0"?>
<sdf version="1.5">
<model name="ultrasonic">
<static>true</static>
<link name="link">
<inertial>
<mass>0.1</mass>
<inertia>
<ixx>0.000166667</ixx>
<iyy>0.000166667</iyy>
<izz>0.000166667</izz>
</inertia>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</visual>
<sensor name="ultrasonic_1" type="ray">
<pose>0.05 0 0 0 0 0</pose>
<always_on>true</always_on>
<visualize>true</visualize>
<update_rate>10</update_rate>
<ray>
<scan>
<horizontal>
<samples>5</samples>
<resolution>1.000000</resolution>
<min_angle>-0.12</min_angle>
<max_angle>0.12</max_angle>
</horizontal>
<vertical>
<samples>5</samples>
<resolution>1.000000</resolution>
<min_angle>-0.01</min_angle>
<max_angle>0.01</max_angle>
</vertical>
</scan>
<range>
<min>0.01</min>
<max>4</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="ultrasonic_sensor" filename="libgazebo_ros_ray_sensor.so">
<ros>
<remapping>~/out:=ultrasonic_sensor_1</remapping>
</ros>
<output_type>sensor_msgs/Range</output_type>
<radiation_type>ultrasound</radiation_type>
<frame_name>link</frame_name>
</plugin>
</sensor>
</link>
</model>
</sdf>
launch#
launch/ultrasonic.launch.py
import os
from launch import LaunchDescription
from launch.actions import AppendEnvironmentVariable, IncludeLaunchDescription, DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
import xacro
PACKAGE_NAME = "ros2_gazebo_tutorial"
WORLD = "camera.world"
MODEL = "ultrasonic"
SDF = "model.sdf"
def generate_launch_description():
pkg = get_package_share_directory(PACKAGE_NAME)
pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
model_sdf_full_path = os.path.join(pkg, "models", MODEL, "model.sdf")
sim_time = LaunchConfiguration("sim_time")
arg_sim_time = DeclareLaunchArgument("sim_time", default_value="true")
resources = [
os.path.join(pkg, "worlds")
]
resource_env = AppendEnvironmentVariable(name="GAZEBO_RESOURCE_PATH", value=":".join(resources))
start_gazebo_server_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
launch_arguments={
"verbose": "true",
'world': WORLD}.items())
start_gazebo_client_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')))
robot_description_path = os.path.join(pkg, "models", MODEL, SDF)
doc = xacro.parse(open(robot_description_path))
xacro.process_doc(doc)
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[
{
'use_sim_time': sim_time,
'robot_description': doc.toxml()
}
]
)
spawn_entity_cmd = Node(
package="gazebo_ros",
executable="spawn_entity.py",
arguments=['-entity', "robot_name_in_model",
'-file', model_sdf_full_path,
'-x', "0",
'-y', "0",
'-z', "0.05"],
output='screen')
rviz = Node(
package="rviz2",
executable="rviz2",
arguments=["-d", os.path.join(pkg, "config", "ultrasonic.rviz")],
)
link_tf = Node(
package='tf2_ros',
executable='static_transform_publisher',
name="link2world",
arguments = ["0", "0", "0.05", "0", "0", "0", "world", "link"]
)
ld = LaunchDescription()
ld.add_action(arg_sim_time)
ld.add_action(resource_env)
ld.add_action(start_gazebo_server_cmd)
ld.add_action(start_gazebo_client_cmd)
ld.add_action(robot_state_publisher)
ld.add_action(spawn_entity_cmd)
ld.add_action(rviz)
ld.add_action(link_tf)
return ld
tf#
- Add static tf between world to link (model)
link_tf = Node(
package='tf2_ros',
executable='static_transform_publisher',
name="link2world",
arguments = ["0", "0", "0.05", "0", "0", "0", "world", "link"]
)
Test sensor read#
ultrasonic_demo_.py
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Range
from rclpy.qos import qos_profile_sensor_data
TOPIC_RANGE = "/ultrasonic_sensor_1"
class MyNode(Node):
def __init__(self):
node_name="ultrasonic_demo"
super().__init__(node_name)
self.create_subscription(Range, TOPIC_RANGE, self.__range_handler, qos_profile=qos_profile_sensor_data)
self.get_logger().info("Hello ultrasonic")
def __range_handler(self, msg: Range):
self.get_logger().info(str(msg.range))
def main(args=None):
rclpy.init(args=args)
node = MyNode()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
run#
run again#
out of range reading
When sensor reading is out of range:
- The visual marker turn brighter
- Rviz stop show marker
- BUG: subscriber stop read data from topic (for output_type sensor_msgs/Range)
echo topic
ros2 topic echo /ultrasonic_sensor_1
Unable to convert call argument to Python object (compile in debug mode for details)
